//
// Created by ubuntu on 22-9-1.
//
#include <ros/ros.h>
#include <dual_arm_msgs/Hand_Seq.h>
#include <dual_arm_msgs/Hand_Force.h>
#include <dual_arm_msgs/MoveJ.h>
#include <dual_arm_msgs/Plan_State.h>



ros::Publisher moveJ_left_pub, moveJ_right_pub, hand_posture_left_pub, hand_posture_right_pub;
dual_arm_msgs::Hand_Seq Hand_Seq;
dual_arm_msgs::MoveJ moveJ_target,moveJ_target_l;

int step_l = 0;
int r_step = 0;




void l_planStateCallback(const dual_arm_msgs::Plan_State::ConstPtr &msg)
{
    // 将接收到的消息打印出来，显示是否执行成功
    if(msg->state)
    {
	 	step_l++;
		ROS_INFO("step:%d", step_l);
		//左臂到达中间位置，发布话题，使其到达桌面
    	if(step_l == 1)
    	{
			ros::Duration(1.0).sleep();
    		moveJ_target_l.joint[0] = 4.312/180*3.14;
    		moveJ_target_l.joint[1] = -31.618/180*3.14;
   			moveJ_target_l.joint[2] = -54.638/180*3.14;
    		moveJ_target_l.joint[3] = -66.652/180*3.14;
    		moveJ_target_l.joint[4] = 135.288/180*3.14;
    		moveJ_target_l.joint[5] = 46.216/180*3.14;
    		moveJ_target_l.joint[6] = 13.536/180*3.14;
    		moveJ_target_l.speed = 0.1;
    		moveJ_left_pub.publish(moveJ_target_l);
			//step=20;
    	}
    	//左臂到达桌面，发布话题，控制机械手抓取杯子
    	else if(step_l == 2)
    	{
    		ros::Duration(1.0).sleep();
			Hand_Seq.seq_num = 2;
    		hand_posture_left_pub.publish(Hand_Seq);
    		//ros::Duration(1.5).sleep();
    	}
    	//抓取杯子完成，发布话题，控制左臂抬起
    	else if(step_l == 3)
    	{
    		ros::Duration(3.0).sleep();
    		moveJ_target_l.joint[0] = 4.698/180*3.14;
    		moveJ_target_l.joint[1] = 0.659/180*3.14;
   			moveJ_target_l.joint[2] = -66.542/180*3.14;
    		moveJ_target_l.joint[3] = -90.69/180*3.14;
    		moveJ_target_l.joint[4] = 128.412/180*3.14;
    		moveJ_target_l.joint[5] = 36.303/180*3.14;
    		moveJ_target_l.joint[6] = -0.881/180*3.14;
    		moveJ_target_l.speed = 0.3;
    		moveJ_left_pub.publish(moveJ_target_l);
			ros::Duration(1.5).sleep();
    	}
    	
    	//左臂回到桌面
    	else if(step_l == 4)
    	{
    		ros::Duration(1.0).sleep();
    		while(r_step != 7);
    		moveJ_target_l.joint[0] = 4.312/180*3.14;
    		moveJ_target_l.joint[1] = -31.618/180*3.14;
   			moveJ_target_l.joint[2] = -54.638/180*3.14;
    		moveJ_target_l.joint[3] = -66.652/180*3.14;
    		moveJ_target_l.joint[4] = 135.288/180*3.14;
    		moveJ_target_l.joint[5] = 46.216/180*3.14;
    		moveJ_target_l.joint[6] = 13.536/180*3.14;
    		moveJ_target_l.speed = 0.3;
    		moveJ_left_pub.publish(moveJ_target_l);
    	}
    	//左臂松开水瓶
    	else if(step_l == 5)
    	{
    		ros::Duration(1.0).sleep();
			Hand_Seq.seq_num = 14;
    		hand_posture_left_pub.publish(Hand_Seq);
    	}
    	
    } 
    else 
	{
        ROS_INFO("*******Plan State Fail");
    }

}


void r_planStateCallback(const dual_arm_msgs::Plan_State::ConstPtr &msg)
{
    // 将接收到的消息打印出来，显示是否执行成功
    if(msg->state)
    {
	 	r_step++;
		ROS_INFO("step:%d", r_step);
		
		if(r_step == 1)
    	{
    		ros::Duration(1.0).sleep();
    		moveJ_target.joint[0] = -164.115/180*3.14;
			moveJ_target.joint[1] = 62.876/180*3.14;
			moveJ_target.joint[2] = -90.458/180*3.14;
			moveJ_target.joint[3] = -61.616/180*3.14;
			moveJ_target.joint[4] = 127.379/180*3.14;
			moveJ_target.joint[5] = 8.031/180*3.14;
			moveJ_target.joint[6] = 252.984/180*3.14;
			moveJ_target.speed = 0.1;
			moveJ_right_pub.publish(moveJ_target);
			//step=20;
    	}
    	//控制右臂到达桌面
    	if(r_step == 2)
    	{
    		ros::Duration(1.0).sleep();
    		moveJ_target.joint[0] = -164.115/180*3.14;
    		moveJ_target.joint[1] = 62.876/180*3.14;
   			moveJ_target.joint[2] = -90.458/180*3.14;
    		moveJ_target.joint[3] = -61.616/180*3.14;
    		moveJ_target.joint[4] = 127.379/180*3.14;
    		moveJ_target.joint[5] = 8.031/180*3.14;
    		moveJ_target.joint[6] = 252.984/180*3.14;
    		moveJ_target.speed = 0.1;
    		moveJ_right_pub.publish(moveJ_target);
			//step=20;
    	}
    	//抓取水瓶
    	else if(r_step == 3)
    	{
    		ros::Duration(1.0).sleep();
			Hand_Seq.seq_num = 2;
    		hand_posture_right_pub.publish(Hand_Seq);
    		//ros::Duration(2.5).sleep();
    		//step=20;
			
    	}
    	//右臂抓起水瓶，移动到空中
    	else if(r_step == 4)
    	{
    		ros::Duration(1.0).sleep();
    		moveJ_target.joint[0] = -98.632/180*3.14;
    		moveJ_target.joint[1] = 50.723/180*3.14;
   			moveJ_target.joint[2] = -158.348/180*3.14;
    		moveJ_target.joint[3] = -66.79/180*3.14;
    		moveJ_target.joint[4] = 99.466/180*3.14;
    		moveJ_target.joint[5] = 9.9/180*3.14;
    		moveJ_target.joint[6] = 334.863/180*3.14;
    		moveJ_target.speed = 0.3;
    		moveJ_right_pub.publish(moveJ_target);
			//step=20;
    	}
    	//倒水
    	else if(r_step == 5)
    	{
    		ros::Duration(1.0).sleep();
    		moveJ_target.joint[0] = -98.632/180*3.14;
    		moveJ_target.joint[1] = 50.723/180*3.14;
   			moveJ_target.joint[2] = -158.348/180*3.14;
    		moveJ_target.joint[3] = -66.79/180*3.14;
    		moveJ_target.joint[4] = 99.466/180*3.14;
    		moveJ_target.joint[5] = 9.9/180*3.14;
    		moveJ_target.joint[6] = 246.305/180*3.14;
    		moveJ_target.speed = 0.1;
    		moveJ_right_pub.publish(moveJ_target);
    	}
    	//等待倒水完成
    	else if(r_step == 6)
    	{
    		ros::Duration(3.0).sleep();
    		moveJ_target.joint[0] = -98.632/180*3.14;
    		moveJ_target.joint[1] = 50.723/180*3.14;
   			moveJ_target.joint[2] = -158.348/180*3.14;
    		moveJ_target.joint[3] = -66.79/180*3.14;
    		moveJ_target.joint[4] = 99.466/180*3.14;
    		moveJ_target.joint[5] = 9.9/180*3.14;
    		moveJ_target.joint[6] = 334.863/180*3.14;
    		moveJ_target.speed = 0.3;
    		moveJ_right_pub.publish(moveJ_target);
    		//step=20;
    	}
    	//右臂回到桌面
    	else if(r_step == 7)
    	{
    		ros::Duration(1.5).sleep();
    		moveJ_target.joint[0] = -140.917/180*3.14;
    		moveJ_target.joint[1] = 40.252/180*3.14;
   			moveJ_target.joint[2] = -177.496/180*3.14;
    		moveJ_target.joint[3] = -60.088/180*3.14;
    		moveJ_target.joint[4] = 96.969/180*3.14;
    		moveJ_target.joint[5] = -46.175/180*3.14;
    		moveJ_target.joint[6] = 343.889/180*3.14;
    		moveJ_target.speed = 0.3;
    		moveJ_right_pub.publish(moveJ_target);
    	}
    	
    	//右臂松开水瓶
    	else if(r_step == 8)
    	{
    		ros::Duration(1.0).sleep();
			Hand_Seq.seq_num = 16;
    		hand_posture_right_pub.publish(Hand_Seq);
    	}

        ROS_INFO("*******Plan State OK");
    } 
    else 
	{
        ROS_INFO("*******Plan State Fail");
    }

}






int main(int argc, char** argv)
{
    ros::init(argc, argv, "api_hand_demo");
    ros::NodeHandle nh;

    // 声明spinner对象，参数2表示并发线程数，默认处理全局Callback队列
    ros::AsyncSpinner spin(2);
    // 启动两个spinner线程并发执行可用回调 
    spin.start();


    /*
     * 1.相关初始化
     */
    // 空间规划指令Publisher
    
    moveJ_left_pub = nh.advertise<dual_arm_msgs::MoveJ>("/l_arm/MoveJ_Cmd", 10);
	moveJ_right_pub = nh.advertise<dual_arm_msgs::MoveJ>("/r_arm/MoveJ_Cmd", 10);
	hand_posture_left_pub = nh.advertise<dual_arm_msgs::Hand_Seq>("/l_arm/Hand_SetSeq", 10);
	hand_posture_right_pub = nh.advertise<dual_arm_msgs::Hand_Seq>("/r_arm/Hand_SetSeq", 10);


    // 订阅机械臂执行状态话题
    ros::Subscriber l_planState_sub = nh.subscribe("/l_arm/Plan_State", 10, l_planStateCallback);
	ros::Subscriber r_planState_sub = nh.subscribe("/r_arm/Plan_State", 10, r_planStateCallback);

  
    ros::Duration(2.0).sleep();

    /*
     * 2.控制机械臂运动到目标位姿
     */
    //定义一个MoveJ指令的目标位姿
    moveJ_target_l.joint[0] = 4.351/180*3.14;
    moveJ_target_l.joint[1] = -17.009/180*3.14;
    moveJ_target_l.joint[2] = -60.696/180*3.14;
    moveJ_target_l.joint[3] = -55.262/180*3.14;
    moveJ_target_l.joint[4] = 105.074/180*3.14;
    moveJ_target_l.joint[5] = 46.146/180*3.14;
    moveJ_target_l.joint[6] = 13.528/180*3.14;
    moveJ_target_l.speed = 0.3;
    
    
    
    moveJ_target.joint[0] = -140.874/180*3.14;
	moveJ_target.joint[1] = -11.21/180*3.14;
	moveJ_target.joint[2] = -175.958/180*3.14;
	moveJ_target.joint[3] = -98.261/180*3.14;
	moveJ_target.joint[4] = 92.213/180*3.14;
	moveJ_target.joint[5] = -36.395/180*3.14;
	moveJ_target.joint[6] = 343.723/180*3.14;
	moveJ_target.speed = 0.3;
	moveJ_right_pub.publish(moveJ_target);
    
	
	

	//ros::Duration(1.0).sleep();
	
    //发布空间规划指令使机械臂运动到目标位姿
    moveJ_left_pub.publish(moveJ_target_l);

    ros::waitForShutdown();

    return 0;
}
